function [x_1,z_1,dx_1,dz_1] = PathPlan(A,B,R,dt)
% This function
% Name          :PathPlan
% Descriptions  :三维空间管道圆柱面上两点间的路径规划
% Argument(s)   :A,B分别为管道上的起点和终点坐标；R为管道的半径
% Return(s)     :目标轨迹，分解为沿管道坐标系xyz轴三个方向的离散坐标点x1，y1，z1
% Author        :LHP, on 4/15/2022
%% 三维目标点转换到平面
% 目标点A B
%A=[0;4;0];
%B=[0;0;10];

x_1 = [];
z_1 = [];
dx_1 = [];
dz_1 = [];

% 转换到平面a b
if A(2)==2*R
    x1=pi*R;
else
    x1=asin(A(1)/R)*R;
end
if B(2)==2*R
    x2=pi*R;
else
    x2=asin(B(1)/R)*R;
end
a=[x1;A(3)];
b=[x2;B(3)];
%% 将目标速度限制在0.3m/s左右
v = 0.3;
s = sqrt((b(1)-a(1))^2+(b(2)-a(2))^2);
T = s/v;
%% 展开二维平面两点间直线路径计算
for t=0:dt:T
%     x_1=(b(1)-a(1))*t/T+a(1);
%     z_1=(b(2)-a(2))*t/T+a(2);
    % dx_1 = (b(1)-a(1))/T;
    % dz_1 = (b(2)-a(2))/T;
    x_1 = [x_1, (b(1)-a(1))*t/T+a(1)];
    z_1 = [z_1, (b(2)-a(2))*t/T+a(2)];
    dx_1 = [dx_1,(b(1)-a(1))/T];
    dz_1 = [dz_1,(b(2)-a(2))/T];
end
